#pragma once

#include <iostream>
#include <fstream>
#include <array>
#include <map>
#include <vector>
#include <algorithm> 
#include <cstdio>
#include <utility>
#include <iterator>
#include <unordered_map>

template<typename T>
int FscanfOrDie(FILE *fptr, const char *format, T* value)
{
    return fscanf(fptr, format, value);
}

class GroundTruthReader
{
    typedef std::array<double, 12 /*pose dimension*/> PoseType;
    typedef std::array<double, 2 /*observation dimension*/> ObservationType;
    typedef std::array<double, 5 /*landmark dimension*/> LandMarkType;
    typedef std::pair<size_t /*id for frame vertex*/, size_t /*id for landmark vertex*/> EdgeType;
    typedef std::map<EdgeType, ObservationType> ObservationsContainer;

  public:
    GroundTruthReader() = delete;
    virtual ~GroundTruthReader() = default;

    GroundTruthReader(std::string foldername)
    {
        // mFramePosesGroundTruth.reserve(2000);
        // mLandMarkGroundTruth.reserve(2000);
        auto posefilename = foldername + "/GroundTruth/GroundTruthposeValue.txt";
        FILE *fptr = fopen(posefilename.c_str(), "r");
        while (!feof(fptr))
        {
            int id;
            FscanfOrDie(fptr, "%d", &id);
            // first read the pose for this frame.
            PoseType /*std::array<double,12>*/ temPose;
            // std::cout << "reading poses.. " << std::endl;
            std::for_each(temPose.begin(), temPose.end(), [&](double &data) {
                FscanfOrDie(fptr, "%lf", &data);
            });

            mFramePosesGroundTruth.emplace(std::make_pair((size_t)id, temPose));
        }

        fclose(fptr);

        auto LandMarkFileName = foldername + "/GroundTruth/GroundTruthLandMarkValue.txt";
        FILE *fptr2 = fopen(LandMarkFileName.c_str(), "r");

        while (!feof(fptr2))
        {
            int id;
            FscanfOrDie(fptr2, "%d", &id);
            // first read the pose for this frame.
            LandMarkType /*std::array<double,12>*/ temLandmark;
            // std::cout << "reading poses.. " << std::endl;
            std::for_each(temLandmark.begin(), temLandmark.end(), [&](double &data) {
                FscanfOrDie(fptr2, "%lf", &data);
            });

            mLandMarkGroundTruth.emplace(std::make_pair((size_t)id, temLandmark));
        }

        fclose(fptr2);

    }

    void PrintGT()
    {
        std::for_each(mFramePosesGroundTruth.begin(), mFramePosesGroundTruth.end(),[&](const std::map<size_t, PoseType >::value_type& mem){
            std::cout << mem.first << ", ";
            std::copy(mem.second.cbegin(), mem.second.cend(),  std::ostream_iterator<double>(std::cout, " ") );
            std::cout << std::endl;
        });

        std::for_each(mLandMarkGroundTruth.begin(), mLandMarkGroundTruth.end(),[&](const std::map<size_t, LandMarkType >::value_type& mem){
            std::cout << mem.first << ", ";
            std::copy(mem.second.cbegin(), mem.second.cend(),  std::ostream_iterator<double>(std::cout, " ") );
            std::cout << std::endl;
        });
    }

  public:
    std::map<size_t, PoseType > mFramePosesGroundTruth;
    std::map<size_t, LandMarkType > mLandMarkGroundTruth;
};